import datetime
import time

import cv2
import numpy as np
import pyzed.sl as sl

from GlobalValues import globalVal, CameraVal


def Video():
    # CameraVal.zed = sl.Camera()
    # # Set configuration parameters
    # CameraVal.init_params = sl.InitParameters()
    # CameraVal.init_params.camera_resolution = sl.RESOLUTION.HD1080  # Use HD720 video mode
    # CameraVal.init_params.camera_fps = 30  # Set fps at 60
    # # init_params.enable_image_enhancement = True
    #
    # # Open the camera
    # err = CameraVal.zed.open(CameraVal.init_params)
    # if err != sl.ERROR_CODE.SUCCESS:
    #     print(repr(err))
    #     exit(-1)
    #
    # CameraVal.runtime_parameters = sl.RuntimeParameters()
    # CameraVal.runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD
    #
    # image_left = sl.Mat()
    # image_right = sl.Mat()
    while True:
        if CameraVal.zed.grab(
                CameraVal.runtime_parameters) == sl.ERROR_CODE.SUCCESS:  # A new image is available if grab() returns SUCCESS
            # Display a pixel color
            CameraVal.zed.retrieve_image(CameraVal.image_left, sl.VIEW.LEFT)  # Get the left image
            cur_image_left = CameraVal.image_left.get_data()[:, :, :3]
            CameraVal.zed.retrieve_image(CameraVal.image_right, sl.VIEW.RIGHT)  # Get the right image
            cur_image_right = CameraVal.image_right.get_data()[:, :, :3]

            if globalVal.sl_VIEW == 'LEFT':
                cur_image = cur_image_left
            elif globalVal.sl_VIEW == 'RIGHT':
                cur_image = cur_image_right
            else:
                cur_image = merge_image(cur_image_left, cur_image_right)
            # 保持最后一帧
            globalVal.mfVideo_rs = cur_image
            globalVal.live_frame = cur_image

            globalVal.frame_queue.put(cur_image)

            # 负责移除队列中的旧图
            globalVal.frame_queue.get() if globalVal.frame_queue.qsize() > 1 else time.sleep(0.01)


def merge_image(cur_image_left, cur_image_right):
    h1, w1, c1 = cur_image_left.shape
    h2, w2, c2 = cur_image_right.shape
    if c1 != c2:
        print("channels NOT match, cannot merge")
        return
    else:
        image3 = np.hstack([cur_image_left, cur_image_right])
        image3 = add_timestamp_wm(image3)
    return image3


def add_timestamp_wm(img):
    t = time.time()
    ts = int(round(t * 1000))
    ts_str = str(ts)
    # 块大小
    block_size = len(img[0]) // 13
    block_height = 4
    for i in range(0, len(ts_str)):
        number = ts_str[i]
        color_number = 255 // 10 * int(number)
        block = np.zeros([block_height, block_size, 3], np.uint8)
        block[::, ::, ::] = color_number
        img[0:block_height, i * block_size:i * block_size + block_size] = block
    font = cv2.FONT_HERSHEY_SIMPLEX
    datet = str(datetime.datetime.now())
    frame = cv2.putText(img, datet, (10, 50), font, 0.8,
                        (255, 255, 255), 2, cv2.LINE_AA)
    return frame
